// function prototype
void wait1ms(void);
#define F_CPU 20000000UL	      // Sets up the default speed for delay.h

// Include definition file
#include <ioavr.h>
#include <inavr.h>
#include <STDIO.H>
#include <STDLIB.H>
#include <string.H>
#include "USART2.h"
//#include "USI_TWI_Master.h"


/// Typedefs //////////
typedef unsigned char u8;
typedef unsigned int u16;
typedef unsigned long u32;

#define _BV(x) (1<<x)

#define ACCEL_ADDR  0x1d
#define MESSAGEBUF_SIZE       4

signed int Vx,Vy,Vz,Ax,Ay,Az,c;
int t,reads,faults;

unsigned char TWI_Act_On_Failure_In_Last_Transmission ( unsigned char TWIerrorMsg )
{
                    // A failure has occurred, use TWIerrorMsg to determine the nature of the failure
                    // and take appropriate actions.
                    // Se header file for a list of possible failures messages.

  __no_operation();
                     
  return TWIerrorMsg; 
}
/* reverse:  reverse string s in place */
void reverse(char s[])
{
    int c, i, j;

    for (i = 0, j = strlen(s)-1; i<j; i++, j--) {
        c = s[i];
        s[i] = s[j];
        s[j] = c;
    }
}

/* itoa:  convert n to characters in s */
void itoa(int n, char s[])
{
    int i, sign;

    if ((sign = n) < 0)  /* record sign */
        n = -n;          /* make n positive */
    i = 0;
    do {       /* generate digits in reverse order */
        s[i++] = n % 10 + '0';   /* get next digit */
    } while ((n /= 10) > 0);     /* delete it */
    if (sign < 0)
        s[i++] = '-';
    s[i] = '\0';
    reverse(s);
} 

void print(char *str)
{
	int i=0;
	while (str[i]!=0){
		USART_Transmit(str[i]);
		i++;
	}
}

void sleep(float msPeriod){
	for (int i=0;i<msPeriod;i++)
		wait1ms();
}

enum { UP, DOWN };

void SPI_MasterInit(void)
{
  volatile char IOReg;
  // set PB2(/SS), PB3(MOSI), PB5(SCK) as output
  DDRB = (1<<PB2)|(1<<PB3)|(1<<PB5);
  PORTB = 0;
  /* Enable SPI, Master, set clock rate fck/16 */
  SPCR = (1<<SPE)|(1<<MSTR)|(1<<CPOL)|(1<<CPHA)|(1<<SPR0)|(1<<SPR1);
  IOReg   = SPSR;                 	// clear SPIF bit in SPSR
  IOReg   = SPDR;
}

char SPI_MasterTransmit(char cData)
{
  /* Start transmission */
  SPDR = cData;
  /* Wait for transmission complete */
  while(!(SPSR & (1<<SPIF)));
  return SPDR;
}

// reads a register
char read_register(char register_name)
{
   char in_byte;
   register_name |= 1<<7;
   PORTB &= ~_BV(PB2);
   SPI_MasterTransmit(register_name);
   in_byte = SPI_MasterTransmit(0);
   PORTB |= _BV(PB2); 
   return in_byte;
}

// write to a register
void write_register(char register_name, char data)
{
   register_name &= ~(0<<7);
   PORTB &= ~_BV(PB2);
   SPI_MasterTransmit(register_name);
   SPI_MasterTransmit(data);
   PORTB |= _BV(PB2);
}

void initCounter1(){
   OCR1A = 10000;
   TCCR1B = (1<<CS10)|(1<<CS11)|(1<<ICES1)|(1<<WGM12); // Timer clock = system clock/64, capture on rising edge
   TIMSK1 = (1<<ICIE1)|(1<<OCIE1A); // Enable Timer1 Capture Event Interrupt, Enable output compare interrupt
   DDRB &= ~(1<<PB0);// Set PE0 as input
   TIFR1 = (1<<ICF1)|(1<<OCF1A); // Clear ICF1 clear pending interrupts
}

void initAccel(){
    write_register(0x20,0);
    write_register(0x20,1<<7|1<<6|0<<5|0<<4|1<<2|1<<1|1<<0);
    write_register(0x21,1<<2);
}

void itoaBinary(int n, char s[])
{
    int i, sign;
    i = 0;
    do {       /* generate digits in reverse order */
        s[i++] = n % 2 + '0';   /* get next digit */
        n /= 2;
    } while (i<8);     /* delete it */
    s[i++] = 'b';
    s[i++] = '0';
    s[i] = '\0';
    reverse(s);
} 

void main()
{
  DDRD = 255;	
  PORTD &= ~_BV(7); 
  char str[16];
  Vx=Vy=Vz=Ax=Ay=Az=0;
  __enable_interrupt();
  USART_Init(129);
  print("\r\nInitializing...\r\n");
  /*
  print("\r\nLogic Analyzer\r\n");
  DDRC = 0;	
  u8 c0,c1,c2,c3,c4,c5;
  u8 prevPC,currPC;
  c0=c1=c2=c3=c4=c5=0;
  prevPC=0;
  while(1){
    currPC=PINC;
    if (prevPC!=currPC){
      itoaBinary(currPC,str);
      print(str);
      print("\r\n");
      prevPC=currPC;
    }


    
    //PORTD |= _BV(7);  
    //sleep(1000);
    //PORTD &= ~_BV(7);
    //sleep(1000);
  }
 */
  
  while(1){
    DDRC = 255;
    PORTC = 0;
    #define BLUE     _BV(PC5)
    #define BLACK    _BV(PC4)
    #define RED      _BV(PC3)
    #define YELLOW   _BV(PC2)
    PORTC = BLUE;
    sleep(10);
    PORTC = BLACK;
    sleep(10);
    PORTC = RED;
    sleep(10);
    PORTC = YELLOW;
    sleep(10);

    

  }
  
  SPI_MasterInit();
  PORTD |= _BV(7);  
  
  sleep(1000);
  PORTD &= ~_BV(7); 
  print("Running.\r\n");
  
  Vx=Vy=Vz=0;
  c=0;
  reads=faults=0;
  initAccel();
  initCounter1();
  read_register(0x28);
  read_register(0x29);
  read_register(0x2a);
  read_register(0x2b);
  read_register(0x2c);
  read_register(0x2d);
  
  char result;

  result=read_register(0xf);
  itoa(result,str);
  print(str);

  while (1)
  {
    itoa(t,str);
    print("last t:");
    print(str);
    itoa(Vx,str);
    print(" Vx:");
    print(str);
    itoa(Ax,str);
    print(" Ax:");
    print(str);
    itoa(Vy,str);
    print(" Vy:");
    print(str);
    itoa(Ay,str);
    print(" Ay:");
    print(str);
    itoa(Vz,str);
    print(" Vz:");
    print(str);
    itoa(Az,str);
    print(" Az:");
    print(str);
    itoa(reads,str);
    print(" reads:");
    print(str);
    itoa(faults,str);
    print(" faults:");
    print(str);
    print("\r\n");
    sleep(500);
  }
    /*while(1){
  print(">");
      u8 rcv=USART_Receive();
      if (rcv>='0' && rcv<='9'){
              USART_Transmit(rcv);
              PORTD = ~_BV(rcv-'0');
              //OCR = (rcv-'0')*100;
      }
       print("\r\n");
  };*/
}

#pragma vector=TIMER1_CAPT_vect
__interrupt void TIMER1_CAPT (void){
    t=ICR1;
    reads++;
    u8 x1,x2;
    TCNT1=0;
    x1=read_register(0x28);
    x2=read_register(0x29);
    Ax=x1|x2<<8;
    Vx+=(Ax*(t>>7))>>11;
    
    x1=read_register(0x2a);
    x2=read_register(0x2b);

    Ay=x1|x2<<8;
    Vy+=(Ay*(t>>7))>>11;
    
    x1=read_register(0x2c);
    x2=read_register(0x2d);
    
    Az=(x1|x2<<8)-1024;
    Vz+=(Az*(t>>7))>>11;
    TCNT1=0;
    c++;
}

#pragma vector=TIMER1_COMPA_vect
__interrupt void TIMER0_COMPA (void){
    faults++;
    read_register(0x28);
    read_register(0x29);
    read_register(0x2a);
    read_register(0x2b);
    read_register(0x2c);
    read_register(0x2d); 
    TCNT1=0;
}


// *** Primitive wait() function ***
void wait1ms(void){
  __delay_cycles(F_CPU/1000);
}
